#include "AdgustSpeed.h"
#include <math.h>
#include "drv_pwmMotors.h"
#include "encode.h"
extern int inc_encode_acc;
/**************************************
 * 功能：计算平方根
 * 参数：
 *       x：计算值
 * 返回：
 *       结果
 * ***********************************/
float InvSqrt(float x)
{
	union
	{
		int32_t i;
		float f;
	} conv;
	conv.f = x;
	conv.i = 0x5f3759df - (conv.i >> 1);
	return 0.5f * conv.f * (3.0f - x * conv.f * conv.f);
}
/**************************************
 * 功能：计算Delta
 * 参数：
 *       handle:结构体句柄
 *       newangle：当前角度
 * ***********************************/
void getDelta(mixer_speed_s *handle)
{
	float S, y, K[2];
	handle->angle += handle->delta;
	if (handle->angle >= (int)handle->motor_drive->encoder.value.Resolution)
		handle->angle -= (int)handle->motor_drive->encoder.value.Resolution;
	else if (handle->angle < 0)
		handle->angle += (int)handle->motor_drive->encoder.value.Resolution;
	handle->delta *= alpha2;
	handle->P[0][0] += handle->P[1][0] + handle->P[0][1] + handle->P[1][1]; //+Q_angle
	handle->P[0][1] = alpha2 * (handle->P[0][1] + handle->P[1][1]);
	handle->P[1][0] = alpha2 * (handle->P[1][0] + handle->P[1][1]);
	handle->P[1][1] = alpha2 * alpha2 * handle->P[1][1] + Q_delta;
	S = handle->P[0][0] + R_measure;
	K[0] = handle->P[0][0] / S;
	K[1] = handle->P[1][0] / S;
	y = handle->rotor.rotor- handle->angle;
	if (y > (int)handle->motor_drive->encoder.value.Resolution / 2)
		y -= handle->motor_drive->encoder.value.Resolution;
	else if (y < -(int)handle->motor_drive->encoder.value.Resolution / 2)
		y += handle->motor_drive->encoder.value.Resolution;
	handle->angle += K[0] * y;
	handle->delta += K[1] * y;
	handle->P[0][0] -= K[0] * handle->P[0][0];
	handle->P[0][1] -= K[0] * handle->P[0][1];
	handle->P[1][0] -= K[1] * handle->P[0][0];
	handle->P[1][1] -= K[1] * handle->P[0][1];
}
#define Pan_Kp_min .3  //.6
#define Pan_Kp_max 1.0 //2.0
#define Pan_Kd_min 0.0
#define Pan_Kd_max 32.0 //64.0
#define Pan_Kp 22.5		//45.0
#define Pan_Ki Pan_Kp * .0045

/**************************************************
 * 功能：混合调速初始化
 * 参数：
 *       handle:调速结构体
 * 返回：
 *       无
 * ***********************************************/
void Mixer_Init(mixer_speed_s *handle)
{
	int ii=0;
	for (ii = 0; ii < 65; ii++)
	{
		handle->TargetDeltaTab[ii] = minTargetDelta * pow(maxTargetDelta / minTargetDelta, (float)ii / 64.0);
	}
	handle->Lead_speed.Kp = 5;
	//handle->Lead_speed.Kd=
	handle->Lead_speed.Ki = 0.5;
	handle->delta = 0;
	handle->P[0][0] = 0;
	handle->P[0][1] = 0;
	handle->P[1][0] = 0;
	handle->P[1][1] = 0;
}
/**********************************************************
 * 功能：设置目标速度
 * 参数：
 * *******************************************************/
void set_TargetSpeed(mixer_speed_s *handle, uint8_t speed)
{
	handle->Target_Delta = handle->TargetDeltaTab[speed];
}
/**********************************************************
 * 功能：计算转子位置
 * 参数：
 *       handle:转子位置计算句柄
 *       value:编码器值
 * 返回：
 *       计算状态
 * *******************************************************/
bool getRotor(mixer_speed_s *handle, uint32_t value)
{
	int temp;
	int Rotor_Sum;
	uint8_t i = 0;
	if (handle->rotor.index < handle->rotor.size)
	{
		handle->rotor.encode_arrray[handle->rotor.index]=value;
		handle->rotor.index++;
		return false;
	}
	else
	{
		handle->rotor.index=0;
		Rotor_Sum = handle->rotor.encode_arrray[handle->rotor.size - 1];
		for (i = 0; i < handle->rotor.size - 1; i++)
		{
			temp = handle->rotor.encode_arrray[i] - handle->rotor.encode_arrray[Encode_Array_Size - 1];
			if (temp > (int)handle->motor_drive->encoder.value.Resolution)
			{
				handle->rotor.encode_arrray[i] -= (int)handle->motor_drive->encoder.value.Resolution;
			}
			else if (temp < -(int)handle->motor_drive->encoder.value.Resolution / 2)
			{
				handle->rotor.encode_arrray[i] += (int)handle->motor_drive->encoder.value.Resolution;
			}
			Rotor_Sum += handle->rotor.encode_arrray[i];
		}
		handle->rotor.rotor = (float)Rotor_Sum / (float)Encode_Array_Size;
		if (handle->rotor.rotor >= (int)handle->motor_drive->encoder.value.Resolution)
		{
			handle->rotor.rotor -= (int)handle->motor_drive->encoder.value.Resolution;
		}
		else if (handle->rotor.rotor < 0)
		{
			handle->rotor.rotor += (int)handle->motor_drive->encoder.value.Resolution;
		}
		if(!handle->init_status)
		{
			handle->TargetPos=handle->rotor.rotor;
			handle->init_status=true;
		}
		handle->rotor.rotor=handle->rotor.encode_arrray[Encode_Array_Size - 1];
		return true;
	}
}
/**********************************************************
 * 功能：领跑算法调速
 * 参数：
 *       handle：结构体句柄
 * 返回：无
 * *******************************************************/
float mixer_Speed_fun(mixer_speed_s *handle)
{
	float Pos_err;
	float PID_Power;
	getDelta(handle);
	handle->TargetPos += handle->Target_Delta;
		if (handle->TargetPos < 0)
		{
			handle->TargetPos += (int)handle->motor_drive->encoder.value.Resolution;
		}
		if (handle->TargetPos > (int)handle->motor_drive->encoder.value.Resolution)
		{
			handle->TargetPos -= (int)handle->motor_drive->encoder.value.Resolution;
		}
		Pos_err = handle->TargetPos - handle->rotor.rotor;
		if (Pos_err > (int)handle->motor_drive->encoder.value.Resolution / 2)
		{
			Pos_err -= (int)handle->motor_drive->encoder.value.Resolution;
		}
		else if (Pos_err < -(int)handle->motor_drive->encoder.value.Resolution / 2)
		{
			Pos_err += (int)handle->motor_drive->encoder.value.Resolution;
		}
		PID_Power = (float)(handle->Lead_speed.Ki * Pos_err + handle->Lead_speed.Kp * (Pos_err - handle->Lead_speed.err_last));
		//PID_Power = fabs(PID_Power) > 2 ? SYBOLNUM(PID_Power) * 2 : PID_Power;
		//handle->motor_drive->motor_power += PID_Power;
//		if(handle->motor_drive->motor_power<0)
//		{
//			handle->motor_drive->motor_power=0;
//		}
		handle->Lead_speed.err_last = Pos_err;
		//PID_Power = Pos_err * handle->Min_Kp + handle->Target_Delta * handle->Min_Kd;
//	if (handle->motor_drive->motor_power > handle->motor_drive->MaxPower)
//		handle->motor_drive->motor_power = handle->motor_drive->MaxPower;
	handle->motor_drive->rotor=handle->rotor.rotor;
		return PID_Power;
}

/*********************************************
 * 功能：电机调速
 * 参数：
 *       motor_speed:电机调速结构体
 * 返回：无
 * ******************************************/
float Speed_Adgust(ZY_FOC_Move_t *motor_speed)
{
	uint8_t i = 0;
	float adg_pwm_value = 0;
	float kp=0.0,ki=0.0,kd=0.0;
  kp=motor_speed->velocity_pid.kp/1000.0;
	ki=motor_speed->velocity_pid.ki/1000.0;
	kd=motor_speed->velocity_pid.kd/1000.0;
	if (motor_speed->target_speed != 0)
	{
		motor_speed->motor_drive->EnableStop = false;
	}
	motor_speed->velocity_pid.err = motor_speed->target_speed - motor_speed->current_speed;
	adg_pwm_value = (float)(ki * (float)motor_speed->velocity_pid.err
		+ kp * (float)(motor_speed->velocity_pid.err - motor_speed->velocity_pid.err_last))
		+kd*(motor_speed->velocity_pid.err-motor_speed->velocity_pid.err_last+motor_speed->velocity_pid.err_last_next);

	motor_speed->velocity_pid.err_last_next=motor_speed->velocity_pid.err_last;
	motor_speed->velocity_pid.err_last = motor_speed->velocity_pid.err;
	return adg_pwm_value;
}
/*********************************

*********************************/
float mixer_pid_power = 0.0,speed_pid_power=0;
static uint8_t speed_num = 0;
static float mixer_power=0.0,speed_power=0.0;
void Motor_Speed(ZY_FOC_Move_t *speed,mixer_speed_s *mixer,ZY_Control_t target)
{
  int average_location_err = 0;
	speed->control_model=target.model;
	speed->postion=inc_encode_acc;
	average_location_err=speed->postion-speed->last_postion;
	average_location_err = (abs(average_location_err) < 5) ? 0 : average_location_err;
	speed->current_speed = average_location_err;
	speed->last_postion = speed->postion;
	if(target.model==ZY_Model_torque)
	{
		speed->motor_drive->motor_power= fabs(target.value) > speed->motor_drive->MaxPower ? SYBOLNUM(target.value) * speed->motor_drive->MaxPower : target.value;
	}
	else if(target.model==ZY_Model_Velocity)
	{
		  speed->target_speed=target.value;
			speed_pid_power=Speed_Adgust(speed);
	    speed_power+=speed_pid_power;
	    speed_power = fabs(speed_power) > speed->motor_drive->MaxPower ? SYBOLNUM(speed_power) * speed->motor_drive->MaxPower : speed_power;
	    speed->motor_drive->motor_power = speed_power;
	}
	else if(target.model==ZY_Model_Angle)
	{
		
	}
}
/*
void Get_Pan_Offset()
{
	u16 Rotor;
	s16 Rotor_Delta;
	static u16 Rotor_Pre;
	static u8 OffsetCnt = 0, Offset_Braking = 0;
	static u16 Offset = 0;
	static float Offset_Power = 0.0, Offset_Nominal_Power = LowPanPower;
	static u16 Delta_Cnt = 0;
	static int Delta = 0, Max_Delta = 0;
	Rotor = SPI1_ReadWriteByte1(0xFFFF) & 0x3FFF;
	if (Pan_Target_Dir == left)
	{
		if (Rotor > 0)
			Rotor = 16384 - Rotor;
	}
	Rotor_Delta = Rotor_Pre - Rotor;
	if (Rotor_Delta > 8192)
		Rotor_Delta -= 16384;
	else if (Rotor_Delta < -8192)
		Rotor_Delta += 16384;
	Rotor_Pre = Rotor;
	if (Pan_Offset_Init == 1)
	{
		Pan_Offset_Init = 0;
		OffsetCnt = 0, Offset_Braking = 0;
		Offset = 0;
		Offset_Power = 0.0, Offset_Nominal_Power = LowPanPower;
		Delta_Cnt = 0;
		Delta = 0;
		Max_Delta = 0;
	}
	else
	{
		if (Offset_Braking == 0)
		{
			if (Offset_Power < Offset_Nominal_Power)
				Offset_Power += 1.0;
			else
			{
				Delta_Cnt++;
				Delta += Rotor_Delta;
				if (Delta_Cnt == 500)
				{
					if (Delta > Max_Delta)
					{
						Max_Delta = Delta;
						if (Pan_Target_Dir == left)
						{
							if (OffsetCnt == 0)
								Left_Offset_Low = Offset;
							else if (OffsetCnt == 1)
								Left_Offset_Mid = Offset;
							else
								Left_Offset_High = Offset;
						}
						else
						{
							if (OffsetCnt == 0)
								Right_Offset_Low = Offset;
							else if (OffsetCnt == 1)
								Right_Offset_Mid = Offset;
							else
								Right_Offset_High = Offset;
						}
					}
					Delta_Cnt = 0;
					Delta = 0;
					//Offset+=10;
					Offset += 6;
					if (Offset > max_Pan_Offset)
						Offset_Braking = 1;
				}
			}
		}
		else
		{
			Offset_Power *= .995;
			if (Offset_Power < 0.1)
			{
				Offset_Braking = 0;
				Offset = 0;
				OffsetCnt++;
				if (OffsetCnt == 1)
				{
					if (Pan_Target_Dir == left)
						print3((short)Left_Offset_Low, 'L', (short)(Max_Delta / 100), 'L', (short)LowPanPower, '\n');
					else
						print3((short)Right_Offset_Low, 'R', (short)(Max_Delta / 100), 'L', (short)LowPanPower, '\n');
					Offset_Nominal_Power = MidPanPower;
					Max_Delta = 0;
				}
				else if (OffsetCnt == 2)
				{
					if (Pan_Target_Dir == left)
						print3((short)Left_Offset_Mid, 'L', (short)(Max_Delta / 100), 'M', (short)MidPanPower, '\n');
					else
						print3((short)Right_Offset_Mid, 'R', (short)(Max_Delta / 100), 'M', (short)MidPanPower, '\n');
					Offset_Nominal_Power = HighPanPower;
					Max_Delta = 0;
				}
				else
				{
					if (Pan_Target_Dir == left)
					{
						print3((short)Left_Offset_High, 'L', (short)(Max_Delta / 100), 'H', (short)HighPanPower, '\n');
						Left_Offset_ML_Slope = (Left_Offset_Mid - Left_Offset_Low) / (MidPanPower - LowPanPower);
						Left_Offset_HM_Slope = (Left_Offset_High - Left_Offset_Mid) / (HighPanPower - MidPanPower);
					}
					else
					{
						print3((short)Right_Offset_High, 'R', (short)(Max_Delta / 100), 'H', (short)HighPanPower, '\n');
						Right_Offset_ML_Slope = (Right_Offset_Mid - Right_Offset_Low) / (MidPanPower - LowPanPower);
						Right_Offset_HM_Slope = (Right_Offset_High - Right_Offset_Mid) / (HighPanPower - MidPanPower);
					}
					Pan_OffsetF = 0;
				}
			}
		}
		Rotor += Offset;
		if (Rotor >= 16384)
			Rotor -= 16384;
		Stator_Pan = (float)Rotor / 16384.0 * MaxPanAngle;
		if (Pan_Target_Dir == left)
			SetPan(Stator_Pan, Offset_Power);
		//else SetPan(-Stator_Pan-PI,Offset_Power);
		else
			SetPan(-Stator_Pan, Offset_Power);
	}
}
#define Pan_Keep_Kp (.5)   //(1.0)
#define Pan_Keep_Ki (.005) //(0.01)
#define Pan_Keep_Kd (2.5)  //(5.0)
#define Pan_KdL (.4)	   //0.8
#define Pan_Keep_Kp2 .22   //.425
#define Pan_Keep_Ki2 Pan_Keep_Kp2 * .05
#define Pan_Keep_FilterCoeff (.125)
void Pan_Keep()
{
	u16 i;
	float Rotor, err, Cur_Offset, D_Power;
	s16 temp;
	int Rotor_Sum;
	static float PID_Power, I_Power, I_Power2;
	static float V_Filter, err_1;
	static u16 cnt = 0, Null_cnt = 0, Pos_cnt = 0, Neg_cnt = 0;
	static float err_Filter = 0.0;
	static float D_Power_Pre;
	static s8 zone, zone_Pre;
	Rotor_Sum = SPI1Array[SPI1Array_MaxSuffix];
	for (i = 0; i < SPI1Array_MaxSuffix; i++)
	{
		temp = SPI1Array[i] - SPI1Array[SPI1Array_MaxSuffix];
		{
			if (temp > 8192)
				SPI1Array[i] -= 16384;
			else if (temp < -8192)
				SPI1Array[i] += 16384;
			Rotor_Sum += SPI1Array[i];
		}
	}
	Pan_SPI1Array_Ready = 0;
	Rotor = (float)Rotor_Sum / SPI1Array_Size;
	if (Rotor >= 16384)
		Rotor -= 16384;
	else if (Rotor < 0)
		Rotor += 16384;
	err = Rotor_Pan_Keep - Rotor;
	if (Pan_Keep_InitF == 1)
	{
		Pan_Keep_InitF = 0;
		PID_Power = 0.0;
		I_Power = 0.0;
		I_Power2 = 0.0;
		cnt = 0;
		Null_cnt = 0;
		Pos_cnt = 0;
		Neg_cnt = 0;
		V_Filter = 0.0;
		err_Filter = 0.0;
		D_Power_Pre = 0.0;
	}
	else
	{

		if (err > 8192)
			err -= 16384;
		else if (err < -8192)
			err += 16384;
		err_Filter = err_Filter + (err - err_Filter) * Pan_Keep_FilterCoeff;
		V_Filter = V_Filter + (err - err_1 - V_Filter) * Pan_Keep_FilterCoeff;
		if (err < -3) //-3should right
		{
			zone = 0;
			if (err < -36)
				I_Power = 0.0;
			else
				I_Power += err * Pan_Keep_Ki;
			D_Power = Pan_KdL * D_Power_Pre + V_Filter * Pan_Keep_Kd;
			D_Power_Pre = D_Power;
			PID_Power = -I_Power - err * Pan_Keep_Kp - D_Power;
			if (PID_Power > maxTiltPower)
				PID_Power = maxPanPower;
			if (PID_Power < 55.0)
				Cur_Offset = Right_Offset_Low + Right_Offset_ML_Slope * (PID_Power - LowPanPower);
			else
				Cur_Offset = Right_Offset_Mid + Right_Offset_HM_Slope * (PID_Power - MidPanPower);
			Rotor += Cur_Offset;
			if (Rotor >= 16384)
				Rotor -= 16384;
			Stator_Pan = Rotor / 16384.0 * MaxPanAngle;
			SetPan(-Stator_Pan, PID_Power);
		}
		else if (err > 3)
		{
			zone = 5;
			if (err > 36)
				I_Power = 0.0;
			else
				I_Power += err * Pan_Keep_Ki;
			D_Power = Pan_KdL * D_Power_Pre + V_Filter * Pan_Keep_Kd;
			D_Power_Pre = D_Power;
			PID_Power = I_Power + err * Pan_Keep_Kp + D_Power;
			if (PID_Power > maxPanPower)
				PID_Power = maxPanPower;
			if (PID_Power < 55.0)
				Cur_Offset = Left_Offset_Low + Left_Offset_ML_Slope * (PID_Power - LowPanPower);
			else
				Cur_Offset = Left_Offset_Mid + Left_Offset_HM_Slope * (PID_Power - MidPanPower);
			if (Rotor > 0.0)
				Rotor = 16384 - Rotor;
			Rotor += Cur_Offset;
			if (Rotor >= 16384)
				Rotor -= 16384;
			Stator_Pan = Rotor / 16384.0 * MaxPanAngle;
			SetPan(Stator_Pan, PID_Power);
		}
		else
		{
			if (err_Filter < -0.5) //-.8should right
			{
				zone = 1;
				Neg_cnt++;
				I_Power2 += err_Filter * Pan_Keep_Ki2;
				PID_Power = (-I_Power2 - err_Filter * Pan_Keep_Kp2);
				Cur_Offset = Right_Offset_Low + Right_Offset_ML_Slope * (PID_Power - LowPanPower);
				Rotor += Cur_Offset;
				if (Rotor >= 16384)
					Rotor -= 16384;
				Stator_Pan = Rotor / 16384.0 * MaxPanAngle;
				SetPan(-Stator_Pan, PID_Power);
			}
			else if (err_Filter > 0.5)
			{
				zone = 4;
				Pos_cnt++;
				I_Power2 += err_Filter * Pan_Keep_Ki2;
				PID_Power = (I_Power2 + err_Filter * Pan_Keep_Kp2);
				Cur_Offset = Left_Offset_Low + Left_Offset_ML_Slope * (PID_Power - LowPanPower);
				if (Rotor > 0.0)
					Rotor = 16384 - Rotor;
				Rotor += Cur_Offset;
				if (Rotor >= 16384)
					Rotor -= 16384;
				Stator_Pan = Rotor / 16384.0 * MaxPanAngle;
				SetPan(Stator_Pan, PID_Power);
			}
			else
			{
				Null_cnt++;
				if (err_Filter < 0)
				{
					if (zone != 2)
						zone_Pre = zone;
					zone = 2;
					if (zone_Pre != 1)
					{
						I_Power2 += err_Filter * Pan_Keep_Ki2;
						PID_Power = (-I_Power2 - err_Filter * Pan_Keep_Kp2);
						Cur_Offset = Right_Offset_Low + Right_Offset_ML_Slope * (PID_Power - LowPanPower);
						Rotor += Cur_Offset;
						if (Rotor >= 16384)
							Rotor -= 16384;
						Stator_Pan = Rotor / 16384.0 * MaxPanAngle;
						SetPan(-Stator_Pan, PID_Power);
					}
					else
						I_Power2 += err_Filter * Pan_Keep_Ki2;
				}
				else
				{
					if (zone != 3)
						zone_Pre = zone;
					zone = 3;
					if (zone_Pre != 4)
					{
						I_Power2 += err_Filter * Pan_Keep_Ki2;
						PID_Power = (I_Power2 + err_Filter * Pan_Keep_Kp2);
						Cur_Offset = Left_Offset_Low + Left_Offset_ML_Slope * (PID_Power - LowPanPower);
						if (Rotor > 0.0)
							Rotor = 16384 - Rotor;
						Rotor += Cur_Offset;
						if (Rotor >= 16384)
							Rotor -= 16384;
						Stator_Pan = Rotor / 16384.0 * MaxPanAngle;
						SetPan(Stator_Pan, PID_Power);
					}
					else
						I_Power2 += err_Filter * Pan_Keep_Ki2;
				}
			}
		}
	}
	err_1 = err;
	cnt++;
	if (cnt == 200)
	{
		//print3((short)(err*100.),'e',(short)(err_Filter*100.),'f',(short)(PID_Power*100.),'p');
	}
	if (cnt == 400)
	{
		//print3(Null_cnt,'n',Pos_cnt,'+',Neg_cnt,'\n');
		cnt = 0;
		Null_cnt = 0;
		Pos_cnt = 0;
		Neg_cnt = 0;
	}
}*/
